#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#一条规划，回到middle位置再送食物，会掉电，所以延时后回到middle
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


def scale_trajectory_speed(traj, scale):
       
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
 
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

def move_to_multiple_targets():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', anonymous=True)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []

    # 第1个目标位姿 14/-45/90/-45/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = 0.1038815445000545
    pose1.position.y = 0.05599530875132761
    pose1.position.z = 0.287593164792294
    pose1.orientation.x = -7.443283672575919e-06
    pose1.orientation.y = 7.57226530269504e-05
    pose1.orientation.z = 0.12184966603307737
    pose1.orientation.w = 0.9925485646044253
    waypoints.append(pose1)

    # 第2个目标位姿 28/-45/90/-45/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.08365577107241515
    pose2.position.y = 0.1086784487646846
    pose2.position.z = 0.2876367813618629
    pose2.orientation.x = 1.760144636797022e-06
    pose2.orientation.y = 5.322223642645702e-07
    pose2.orientation.z = 0.24192347154193786
    pose2.orientation.w = 0.970295333346356
    waypoints.append(pose2)

    # 第3个目标位姿 28/-50/90/15/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = -0.035022935822420206
    pose3.position.y = 0.045609591393929116
    pose3.position.z = 0.10616480515032783
    pose3.orientation.x = -0.11166965546205539
    pose3.orientation.y = 0.447983924420112
    pose3.orientation.z = 0.21466435979487009
    pose3.orientation.w = 0.8606738663070932
    waypoints.append(pose3)

    # 4 28/-70/120/-12/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.0059124806471319985
    pose4.position.y = 0.06736960543726173
    pose4.position.z = 0.0851790937420965
    pose4.orientation.x = -0.0787478196797887
    pose4.orientation.y = 0.3158430604116397
    pose4.orientation.z = 0.22880413581912243
    pose4.orientation.w = 0.9174369784990976
    waypoints.append(pose4)

    # 5 28/-65/121/-18/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.018667787233523553
    pose5.position.y = 0.0741529288356771
    pose5.position.z = 0.08468770404950043
    pose5.orientation.x = -0.07873418952429172
    pose5.orientation.y = 0.31589829849271545
    pose5.orientation.z = 0.22879950391761433
    pose5.orientation.w = 0.9174202850473789
    waypoints.append(pose5)

    # 第6个目标位姿 28/-68/128/-27/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.032490128976992244
    pose6.position.y = 0.08148829798108001
    pose6.position.z = 0.08403494444521113
    pose6.orientation.x = -0.0686576561182485
    pose6.orientation.y = 0.275514230486597
    pose6.orientation.z = 0.2319904963985905
    pose6.orientation.w = 0.9303539351432134
    waypoints.append(pose6)

    # 第7个目标位姿 28/-70/135/-36/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.044606693998343105
    pose7.position.y = 0.08792626579373217
    pose7.position.z = 0.08060679535604816
    pose7.orientation.x = -0.06055302337925643
    pose7.orientation.y = 0.24294582246096078
    pose7.orientation.z = 0.23423143748854183
    pose7.orientation.w = 0.9393861252969676
    waypoints.append(pose7)

    # 第8个目标位姿 28/-70/141/-50/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.06420066898792094
    pose8.position.y = 0.098369914979654
    pose8.position.z = 0.09411826462949197
    pose8.orientation.x = -0.044063328860029054
    pose8.orientation.y = 0.17674516776628177
    pose8.orientation.z = 0.2379435127905711
    pose8.orientation.w = 0.9540453099522713
    waypoints.append(pose8)

    # 第9个目标位姿 28/72/150/-78/0 举平
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.08723638964773509
    pose9.position.y = 0.11059034428259178
    pose9.position.z = 0.1531080334994858
    pose9.orientation.x = 3.17781117137612e-05
    pose9.orientation.y = -0.00011987965642704671
    pose9.orientation.z = 0.24193188893662781
    pose9.orientation.w = 0.970293226676645
    waypoints.append(pose9)


    # # 第10个目标位姿 14/72/150/-78/0 举平移
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.10781150531469692
    pose10.position.y = 0.057003964556817374
    pose10.position.z = 0.1530723139619223
    pose10.orientation.x = 9.186180341598707e-06
    pose10.orientation.y = -5.977016834688678e-05
    pose10.orientation.z = 0.12192029901858292
    pose10.orientation.w = 0.9925398919088142
    waypoints.append(pose10)

    # 第11个目标位姿 middle
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.11075168589927481
    pose11.position.y = -1.091277667118024e-05
    pose11.position.z = 0.2875988062936282
    pose11.orientation.x = 1.8382147442206865e-06
    pose11.orientation.y = 8.132110012412454e-05
    pose11.orientation.z = -2.4453694472133078e-05
    pose11.orientation.w = 0.9999999963927584
    waypoints.append(pose11)

    # 第12个目标位姿 0/40/0/-40/0
    pose12 = geometry_msgs.msg.Pose()
    pose12.position.x = 0.3035704582748756
    pose12.position.y = -4.6019088696090045e-05
    pose12.position.z = 0.30536264212765296
    pose12.orientation.x = 1.8345570569793093e-06
    pose12.orientation.y = -7.180863568924419e-05
    pose12.orientation.z = -6.589662785214818e-05
    pose12.orientation.w = 0.9999999952488945
    waypoints.append(pose12)



    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,   # 目标位姿序列
        0.01,        # eef_step，终端执行器的步长
        False)         # jump_threshold，跳跃阈值
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    new_plan=scale_trajectory_speed(plan,1.2)
    arm_group.execute(new_plan, wait=True)

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()
    
    # rospy.sleep(3)
    # arm_group.set_named_target("middle")
    # plan = arm_group.go(wait=True)

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

if __name__ == '__main__':
    try:
        move_to_multiple_targets()
    except rospy.ROSInterruptException:
        pass
